
#include <BSphere>
#include "IOverlapPair.h"
#include "IAlgorithm.h"
#include "ITriangleShape.h"
#include "ISoftConcaveAlgorithm.h"
#include "IContinuousConvexCast.h"
#include "member_BMesh.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

ISoftConcaveAlgorithm::ISoftConcaveAlgorithm()
{

}
ISoftConcaveAlgorithm::~ISoftConcaveAlgorithm()
{

}

void ISoftConcaveAlgorithm::clearCache()
{
	m_softBodyTriangleCallback.clearCache();
}

void ISoftConcaveAlgorithm::caculate()
{
	if (dynamic_cast<BSoftBody*>(_bodyB))
	{
		this->swap();
	}
	BMesh* mesh = dynamic_cast<BMesh*>(_shapeB);
	m_softBodyTriangleCallback.m_softBody = dynamic_cast<BSoftBody*>(_bodyA);
	m_softBodyTriangleCallback.m_trigBody = _bodyB;
	m_softBodyTriangleCallback.setTimeStepAndCounters(mesh->margin(), _matrixB, this);
	mesh_member(mesh)->overlapping(&m_softBodyTriangleCallback, m_softBodyTriangleCallback.space());
}
void ISoftConcaveAlgorithm::timeImpact()
{
	if (dynamic_cast<BSoftBody*>(_bodyB))
	{
		this->swap();
	}

	const BMatrix& matrixA = node_member(_bodyA)->matrix;
	const BMatrix& matrixB = node_member(_bodyB)->matrix;

	const BMatrix& interpMatrixA = body_member(_bodyA)->predictedMatrix;
	const BMatrix& interpMatrixB = body_member(_bodyB)->predictedMatrix;

	CastResult caseResult;
	IContinuousConvexCast ccc(_shapeA, _shapeB);
	ccc.calcTimeOfImpact(matrixA, interpMatrixA, matrixB, interpMatrixB, caseResult);
}
